#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "turtlesim/msg/pose.hpp"
#include <chrono>

using namespace std::chrono_literals;

/*
* 需求：控制乌龟移动到指定目标
* 1. 通过订阅话题获取乌龟当前位置
* 2. 根据乌龟当前位置和目标位置计算乌龟需要移动的距离和方向
* 3. 分别计算角速度和线速度（指定策略）
* 4. 通过发布话题控制乌龟移动
*/

class TurtleControlNode: public rclcpp::Node
{
private:
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_subscription_;

    double target_x_{0.1};  // 目标位置X,设置默认值1.0
    double target_y_{0.1};  // 目标位置Y,设置默认值1.0
    double k_{0.1};         // 比例系数，控制输出=误差*比例系数
    double max_speed_{3.0}; // 最大线速度，设置默认值3.0


    void pose_callback(const turtlesim::msg::Pose::SharedPtr pose)
    {
        double  current_x = pose->x;
        double  current_y = pose->y;
        RCLCPP_INFO(this->get_logger(), "Current position: x=%f, y=%f", current_x, current_y);
        double  distance = sqrt(pow(current_x - target_x_, 2) + pow(current_y - target_y_, 2));
        double  angle = atan2(current_y - target_y_, current_x - target_x_) - pose->theta;

        auto message = geometry_msgs::msg::Twist();
        if (distance > 0.1)
        {
            if (fabs(angle)>0.2)
            {
                message.angular.z = fabs(angle);
            }
            else
            {
                message.linear.x = k_ * distance;
            }
        }

        // 限制最大值
        if (message.linear.x > max_speed_)
        {
            message.linear.x = max_speed_;
        }
        
        publisher_->publish(message);
    }


public:
    explicit TurtleControlNode(const std::string &name): Node(name)
    {
        publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("turtle1/cmd_vel", 10);
        pose_subscription_ = this->create_subscription<turtlesim::msg::Pose>("turtle1/pose", 10, std::bind(&TurtleControlNode::pose_callback, this, std::placeholders::_1));

    }
};


int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<TurtleControlNode>("turtle_control");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
